import struct
import time
import math
from threading import Thread, Event

from Brain.vector import Vector
from Brain.io.core import Comm 
from Brain.operations.module import Module 

class MeasuredPosition:
	""" store position data
	"""
	def __init__(self, r1, r2, ts):
		self.r1 = r1
		self.r2 = r2
		self.time_stamp = ts

class UltrasoundModule(Thread, Module):
	""" absolute position sensor
	"""
	
	LENGTH = 6
	STARTER = '@'
	
	BEACON_DISTANCE = 2144.0
	
	MAX_ERRORS = 10 * 3 
	ALOWED_CHANGE = 250.0

	def __init__(self, world):
		Thread.__init__(self)
		Module.__init__(self)
		self._world = world 
		self._error_counter = 0 
		self._pos = Vector(0, 0)
		self._calculated_angle = Vector(1, 0)
		self.pipe = Comm.link('ultrasound', self._isr)
		
		self._comm_chunk = ''
		self._history = []
		self._process_index = 0
		
		self._event_got_new_position = Event()
		self.new_data_event = Event()
	
	def read(self):
		return self._pos.copy()
	
	def reset_maneuver(self):
		""" used to mark begining of new direction
			breaks history
			NOT USED
		"""
		pass
	
	def reset_pos(self, new_pos ):
		""" force new position """
		self._pos = new_pos.copy()
	
	def _isr(self, data, time_stamp):
		""" Communication interrupt service routine
			- got new chunk of data
			- in separate threadspace, only append to a list
		"""
		self._comm_chunk += str(data)
		while len(self._comm_chunk) >= self.LENGTH:
			if self._comm_chunk[0] == self.STARTER:
				data = self._comm_chunk[1:self.LENGTH]
				#x, y, r1, r2, = struct.unpack('>hhhh', data)
				r1, r2, chksum, = struct.unpack('>hhB', data)
				## !! no checksum calculation ...
				self._comm_chunk = self._comm_chunk[self.LENGTH:]
				record = MeasuredPosition(r1, r2, time_stamp)
				self._history.append( record )
				self._event_got_new_position.set() ## avake waiting thread
			else:
				self._comm_chunk = self._comm_chunk[1:]
		return False
	
	def packit(self, hi, lo):
		ret = ((int(hi))<<8) | (int(lo))
		if hi & 0x80: ret |= 0xFFFF0000
		return ret
	
	def _calculate_position(self, record):
		## 2.9 us to traverse one mm
		## counter is incremented on 3.2us
		## convert to real distance :
		## raw * multiplier + offset
		r1 = (record.r1 * 0.3) + 281.0
		r2 = (record.r2 * 0.3) + 281.0
		## r1 or r2 cannot be greater than 3600 mm ..
		
		## triangulation :
		y = ((r1 * r1) + (self.BEACON_DISTANCE * self.BEACON_DISTANCE) - (r2 * r2)) / (2.0 * self.BEACON_DISTANCE)
		x = math.sqrt( (r1 * r1) - (y * y) )
		return Vector(x, y)
	
	def run(self):
		while self._working:
			while self._working:
				self._event_got_new_position.wait() ## sleep until signaled ..
				self._event_got_new_position.clear()
				
				while 1:
					if len(self._history) > self._process_index:
						record = self._history[self._process_index]
						self._process_index += 1
						##
						## No sensor error check done, accept last coordinate
						##
						pos = self._calculate_position(record)
						
						if (0.0 > pos.x > 3000.0) or (0.0 > pos.y > 2100):
							self._error_counter += 1 
							continue
						
						robot_pos = Vector(self._world.pos_x, self._world.pos_y)
						diff = pos - robot_pos
						if diff.length() > self.ALOWED_CHANGE:
							self._error_counter += 1 
							continue
						
						if self._error_counter > self.MAX_ERRORS: 
							self._working = False 
							self._world.ultrasound_working = False 
							break 
						
						self._pos = pos
						self.new_data_event.set()	## signal to central cortex
						## No angle calculation
					else:
						break
			
			time.sleep(1.5)

	def dump_logs(self):
		now = time.strftime('%m-%d-%H%M')
		f = open('ultrasound_%s.log' % (now, ), 'wb')
		for record in self._history[:]:
			f.write( 'r1: %.2f r2: %s pos: %s\t\tts:%s\n' % (record.r1, record.r2, self._calculate_position(record), record.time_stamp, ) )
		f.close()
		pickle.dump( self._history, open('ultrasound_%s.pickle' % (now, ), 'wb') )

	
if __name__ == '__main__':
	print 'run test unit from tests'




